#pragma once

#include <map>
#include <constants.h>
#include <Plane.h>
#include <PlaneParas.hpp>

#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>

// 语义标签
//#include <constant/label.h>

using namespace std;

class PlaneUtils
{
public:
    PlaneUtils(){}

#if 0
        : su(new ShowUtils){}

    PlaneUtils(std::string name, bool stopBySpace = false)
        : su(new ShowUtils) 
    { 
        su->init(name, stopBySpace); 
    } 

    void wait() { su->waitSpace(); }
    ShowUtils* getShowUtils() { return su;}
#endif

    #if 0
    Plane extractPlane(PointCloud::Ptr cloud, float threshold=0.01)
    {
        //创建分割对象 -- 检测平面参数
		pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); //存储输出的模型的系数
		pcl::PointIndices::Ptr inliers(new pcl::PointIndices);				  //存储内点，使用的点
		pcl::SACSegmentation<PointType> seg;
		//可选设置
		seg.setOptimizeCoefficients(true);
		//必须设置
		seg.setModelType(pcl::SACMODEL_PLANE); //设置模型类型，检测平面
		seg.setMethodType(pcl::SAC_RANSAC);	//设置方法【聚类或随机样本一致性】
		seg.setMaxIterations(1000);
		seg.setDistanceThreshold(threshold);
		seg.setInputCloud(cloud);
		seg.segment(*inliers, *coefficients); //分割操作

		if (inliers->indices.size() == 0)
		{
			PCL_ERROR("Could not estimate a planar model for the given dataset.");
			Plane empty(coefficients->values);
			return empty;
		}
        
		Plane pl(coefficients->values);
		
		pl.setPoints(cloud);
		pl.computeMean();
		pl.computeNormalDirection();
		return pl;
    }
    #endif

    
    void movePlanesToPreviousVector(vector<Plane> &planeVectorPreviousFrame, vector<Plane> &planeVectorCurrentFrame)
    {
        planeVectorPreviousFrame.clear();
        copy(planeVectorCurrentFrame.begin(), planeVectorCurrentFrame.end(), back_inserter(planeVectorPreviousFrame));
        planeVectorCurrentFrame.clear();
    }

// 合并平面
#if 0
    void mergePlanes(vector<Plane> &planeVector, PlaneDetector *planeDetector)
    {
        if (planeVector.size() == 0) return;
        vector<vector<Plane>> clusteredPLanes;
        Clustering clustering;
        clustering.setCutSimilarity(CLUSTERING_MAX_DISTANCE_THRESHOLD);
        clustering.selectParts(planeVector, clusteredPLanes);
        planeVector = Clustering::getAveragedPlanes(clusteredPLanes, planeDetector);
    }
#endif

    // 计算两个平面之间的距离
    double getDistanceBetweenTwoPlanes(const Plane &firstPlane, const Plane &secondPlane)
    {
        if (firstPlane.getPoints().size() == 0 || secondPlane.getPoints().size() == 0)
            return -1;

        random_device rd;
        mt19937 rng(rd());

        double distance = 0;
        vector<Point3D> pointsVec = secondPlane.getPoints();

        uniform_int_distribution<unsigned int> pointIndex(0, secondPlane.getNumberOfPoints() - 1);
        for (int i = 0; i < NUMBER_OF_POINTS_TO_COUNT_DISTANCE; ++i)
        {
            Vector3d randomPointOnSecondPlane = pointsVec[pointIndex(rng)].position;
            distance += firstPlane.getDistanceFromPoint(randomPointOnSecondPlane);
        }

        return distance / NUMBER_OF_POINTS_TO_COUNT_DISTANCE;
    }

    // 判定是否存在至少3个互相垂直的平面
    bool arePlanesValid(vector<Plane> &planes)
    {
        for (Plane &plane1 : planes)
        {
            for (Plane &plane2 : planes)
            {
                double angle12 = plane1.getAngleBetweenTwoPlanes(plane2);
                if (angle12 >= VALID_ANGLE_BETWEEN_PLANES && angle12 <= (180 - VALID_ANGLE_BETWEEN_PLANES))
                {
                    for (Plane &plane3 : planes)
                    {
                        double angle13 = plane3.getAngleBetweenTwoPlanes(plane1);
                        double angle23 = plane3.getAngleBetweenTwoPlanes(plane2);
                        if (angle13 >= VALID_ANGLE_BETWEEN_PLANES && angle13 <= (180 - VALID_ANGLE_BETWEEN_PLANES) && angle23 >= VALID_ANGLE_BETWEEN_PLANES && angle23 <= (180 - VALID_ANGLE_BETWEEN_PLANES))
                        {
                            return true;
                        }
                    }
                }
            }
        }
        return false;
    }

    // 判断互相垂直
    bool arePlanesValid(Plane &plane1, Plane &plane2, Plane &plane3)
    {
        double angle12 = plane1.getAngleBetweenTwoPlanes(plane2);
        double angle13 = plane1.getAngleBetweenTwoPlanes(plane3);
        double angle23 = plane2.getAngleBetweenTwoPlanes(plane3);
        return (angle12 >= VALID_ANGLE_BETWEEN_PLANES && angle12 <= (180 - VALID_ANGLE_BETWEEN_PLANES)) &&
               (angle13 >= VALID_ANGLE_BETWEEN_PLANES && angle13 <= (180 - VALID_ANGLE_BETWEEN_PLANES)) &&
               (angle23 >= VALID_ANGLE_BETWEEN_PLANES && angle23 <= (180 - VALID_ANGLE_BETWEEN_PLANES));
    }


    // 将平面变换到全局坐标系下
    vector<Plane> getTransformedPlanes(vector<Plane> planes, Matrix4d m)
    {
        vector<Plane> transformedPlanes;
        for (auto &p : planes)
        {
            Plane transformed_p = p.getPlaneSeenFromGlobalCamera(m);
            transformedPlanes.push_back(transformed_p);
        }
        return transformedPlanes;
    }

    void tranformPlaneParasAndCenter(vector<Plane> &planes, Matrix4d m)
    {
        for(auto &p : planes)
        {
            p.transformMeanToGlobal(m);
            p.transformParametersToGlobal(m);
        }
    }

    // 获得局部坐标系下的全局平面的观测参数（不转换点云）
    // 其实这里进行了点云的拷贝，还可以进一步优化
    void getPlaneParasSeenFromLocalCamera(
        const vector<Plane>& global, const Matrix4d& last_T,
        vector<PlaneParas>& local_paras)
    {
        for(auto &p : global)
        {
            PlaneParas para(p);
            para.transformParas(last_T.inverse());
            local_paras.push_back(para);
        }
    }

    vector<Plane> getPlanesSeenFromLocalCamera(vector<Plane> &global, Matrix4d last_T)
    {
        Matrix4d last_T_inv = last_T.inverse();
        return getTransformedPlanes(global, last_T_inv);
    }
    
    Vector3d computeMean(const vector<Point3D> &pointsVector) 
    {
        Vector3d mean(0, 0, 0);
        for (auto &vector : pointsVector) {
            mean += vector.position;
        }
        mean /= (double) pointsVector.size();
        return mean;
    }
    // 合并重复的点云
    void mergePlanes(Plane &mergeTo, const Plane &mergeFrom){

        // vector<Point3D> points_new = mergeTo.getPoints();
        // Vector3d temp = computeMean(mergeFrom.getPoints());
        // mergeTo.insertPoints(mergeFrom.getPoints());
        // mergeTo.setMeanPoint( (temp+mergeTo.getMeanPoint())/2.0 );
        //mergeTo.computeMean(mergeTo.getPoints());
        Vector3d m1 = mergeTo.getMeanPoint();
        Vector3d m2 = mergeFrom.getMeanPoint();
        int n1 = mergeTo.getPointCloud()->points.size();
        int n2 = mergeFrom.getPointCloud()->points.size();

        mergeTo.setMeanPoint((n1*m1 + n2*m2)/(n1+n2));
        mergeTo.insertPointCloud(mergeFrom.getPointCloud());
    }

    void printTwoPlanesDiff(Plane p1, Plane p2)
    {
        float degree = p1.getAngleBetweenTwoPlanes(p2);
        // if(degree > 50) return;

        std::cout << "Plane printer - angle: " << degree << ", D: " << abs(p1.getD() - p2.getD()) 
                  << ", mean_dis: " << p1.getMeanDistanceBetweenTwoPlanes(p2) << std::endl; 
        //pLine();
    }

    double get_angle(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)
    {
        return r2d(atan2(v1.cross(v2).norm(), v1.transpose() * v2));
    }

    double get_distance(const Eigen::Vector3d& p1, const Eigen::Vector3d& p2)
    {
        return sqrt( pow(p1.x()-p2.x(), 2.0) 
                + pow(p1.y()-p2.y(), 2.0)
                + pow(p1.z()-p2.z(), 2.0));
    }

    void printTwoPlanesDiff(const PlaneParas& p1, const PlaneParas& p2)
    {
        float degree = get_angle(p1.normal, p2.normal);
        // if(degree > 50) return;

        std::cout << "Paras printer - angle: " << degree << ", D: " << abs(p1.D - p2.D) 
                  << ", mean_dis: " << get_distance(p1.center, p2.center) << std::endl; 
        //pLine();
    }

    int findMatchPlane(const PlaneParas& pl, const vector<PlaneParas>& pls, bool print = false)
    {
        for(int i=0; i<pls.size(); ++i)
        {
            if(print)
                printTwoPlanesDiff(pl, pls[i]);

            if(pl.isEqualTo(pls[i])) 
                return i;

            if(pl.isCloseTo(pls[i]))
            {
                float angle = get_angle(pl.normal, pls[i].normal);
                float d = abs(pl.D - pls[i].D);
                
                // 中心相近，但是平面参数相差较大，可能是帧畸变带来的错误估计
                if(d > D_THRESHOLD_BETWEEN_EQUAL_PLANES) 
                    return -2;

                // 中心相近，但是平面夹角相差较大，可能是帧畸变带来的错误估计
                if(angle > MAX_ANGLE_BETWEEN_EQUAL_PlANES && 
                   angle < 180 - MAX_ANGLE_BETWEEN_EQUAL_PlANES)
                    return -2; 
            }
            
        }
        return -1;
    }

    int findMatchPlane(Plane curPlane, vector<Plane> globalPlanes, bool print = false)
    {
        for(int i=0; i<globalPlanes.size(); ++i)
        {
            auto gP = globalPlanes[i];
            
            if(print)
            {
                printTwoPlanesDiff(curPlane, gP);
                
            }

            if(curPlane.isEqualTo(gP))
            {
                // 这里实际上还要增加一个边缘是否足够靠近的判断,暂时先不管它
                return i;
            }

            if(curPlane.isCloseTo(gP))
            {
                float angle = curPlane.getAngleBetweenTwoPlanes(gP);
                float d = abs(curPlane.getD() - gP.getD());
                
                // 中心相近，但是平面参数相差较大，可能是帧畸变带来的错误估计
                if(d > D_THRESHOLD_BETWEEN_EQUAL_PLANES) 
                    return -2;

                // 中心相近，但是平面夹角相差较大，可能是帧畸变带来的错误估计
                if(angle > MAX_ANGLE_BETWEEN_EQUAL_PlANES && 
                   angle < 180 - MAX_ANGLE_BETWEEN_EQUAL_PlANES)
                    return -2; 
            }
        }
        return -1;
    }

    // 循环地找一对重复的平面
    bool getDuplicatePlanes(vector<Plane> planes, pair<int,int>& duplicate_pair)
    {
        for(int i=0; i<planes.size()-1; ++i)
            for(int j=i+1; j<planes.size(); ++j)
                if(planes[i].isEqualTo(planes[j]))
                {
                    duplicate_pair = make_pair(i, j);
                    return true;
                }
        return false;
    }

    // 移除单帧的平面中，所有重复的平面对（实际上是合并）
    void removeDuplicate(vector<Plane> &planes)
    {
        pair<int, int> duplicates;
        while(getDuplicatePlanes(planes, duplicates))
        {
            // 编号靠后的合并到编号靠前的平面中
            int front = duplicates.first;
            int rear = duplicates.second;
            mergePlanes(planes[front], planes[rear]);
            // 删除后一个平面
            planes.erase(planes.begin()+rear);
        }
    }

#if 0
    //----------------------------------------------------------------------------
    //--------------------------- 下面是一些可视化工具 -------------------------------
    //----------------------------------------------------------------------------
    void visPlan(Plane plane, string show_field = "intensity", int plane_id = _plane_id)
    {
        std::string planeid = "plane" + std::to_string(plane_id);
        
        // 如果已存在，则需要更新
        if(su->getViewer()->contains(planeid)) 
            su->getViewer()->removeShape(planeid);
     
        // 可视化平面内的点云
        Point3DCloud points3D(plane.getPoints());
        PointCloud::Ptr cloud(new PointCloud);
        points3D.toPointCloud(cloud);
        su->ShowCloud(cloud, plane_id, "planecloud", show_field);

        // 可视化平面的质心和平面的法向
        Vector3d center = plane.getMeanPoint();
        visMeanPoint(su->getViewer(), center, plane.getPlaneNormalVec(), plane_id);

        // 可视化平面的编号
        pcl::PointXYZ position(center[0], center[1], center[2]);
        if(su->getViewer()->contains("text" + std::to_string(plane_id))) 
            su->getViewer()->removeShape("text" + std::to_string(plane_id));
        su->getViewer()->addText3D(planeid, position, 0.5, 1.0, 1.0, 1.0, "text" + std::to_string(plane_id), 0);

        // 可视化平面
        pcl::ModelCoefficients coeff;
        coeff.values.push_back(plane.getA());
        coeff.values.push_back(plane.getB());
        coeff.values.push_back(plane.getC());
        coeff.values.push_back(-plane.getD()); // D=-(Ax+By+Cz)
        
        
        su->getViewer()->addPlane(coeff, center(0), center(1), center(2), planeid);
    }

    void visPlans(const vector<Plane> &planes, string show_field = "intensity")
    {
        for (auto plane : planes)
        {
            visPlan(plane, show_field, _plane_id);
            _plane_id++;
        }
    }

    void visGlobalPlanes(const vector<Plane> &planes, string show_field = "curvature")
    {
        for(int i=0; i<planes.size(); ++i)
            visPlan(planes[i], show_field, i);
    }

    void visPose(Eigen::Matrix4d t, long pose_id)
    {
        Eigen::Transform<double, 3, Eigen::Affine> tf(t);
        su->getViewer()->addCoordinateSystem(1.5, (const Eigen::Affine3f)tf, "pose" + std::to_string(pose_id), 0);
        //viewer->addText(std::to_string(pose_id), t(0,3), t(1,3), "text"+std::to_string(pose_id), 0);
        pcl::PointXYZ position;
        position.x = t(0, 3);
        position.y = t(1, 3);
        position.z = t(2, 3);
        su->getViewer()->addText3D(std::to_string(pose_id), position, 0.5, 1.0, 1.0, 1.0, "text" + std::to_string(pose_id), 0);
        su->getViewer()->spinOnce(10);
    }

    void visMatchedPlanes(vector<pair<Plane, Plane>> &matchedPlanes)
    {
        for (auto pairMatch : matchedPlanes)
        {
            vector<Plane> forshow;
            forshow.push_back(pairMatch.first);
            forshow.push_back(pairMatch.second);
            visPlans(forshow);
            wait();
        }
    }

    void visPoseAndPlanes(pcl::visualization::PCLVisualizer *viewer,
                          vector<Plane> &prePlanes, vector<Plane> &curPlanes,
                          Eigen::Matrix4d prePose, Eigen::Matrix4d curPose)
    {
        visPose(prePose, prePlanes[0].getFrameId());
        visPose(curPose, curPlanes[0].getFrameId());

        vector<Plane> transformedPre;
        for (auto &p : prePlanes)
        {
            Plane transformed_p = p.getPlaneSeenFromGlobalCamera(prePose);
            transformedPre.push_back(transformed_p);
        }
        vector<Plane> transformedCur;
        for (auto &p : curPlanes)
        {
            Plane transformed_p = p.getPlaneSeenFromGlobalCamera(curPose);
            transformedCur.push_back(transformed_p);
        }
        int cnt = 0;
        visPlans(transformedPre);
        visPlans(transformedCur);
    }

private:
    void visMeanPoint(pcl::visualization::PCLVisualizer *viewer, Vector3d mean, Vector3d norm, int &planeCnt)
    {
        std::string meanid = "mean" + std::to_string(planeCnt);
        if (viewer->contains(meanid))
            viewer->removeShape(meanid);
        Vector3d end1 = mean + norm;

        pcl::PointXYZ start, end;
        start.x = mean(0);
        start.y = mean(1);
        start.z = mean(2);
        end.x = end1(0);
        end.y = end1(1);
        end.z = end1(2);
        viewer->addArrow<pcl::PointXYZ>(end, start, 1.0, 1.0, 1.0, false, meanid);
    }
#endif
};